#include "TorsionSpring.h"

TorsionSpring::TorsionSpring(Particle* _p1, Particle* _p2, Particle* _p3, Particle* _p4, double _stiffness)
: p1(_p1), p2(_p2), p3(_p3), p4(_p4), stiffness(_stiffness)
{
	restAngle = computeAngle();
}

double TorsionSpring::computeAngle()
{
	Vector3 v1 = (p3->position - p1->position)+(p3->position - p4->position)/2;
	double l1 = v1.length();
	Vector3 v2 = (p2->position - p1->position)+(p2->position - p4->position)/2;
	double l2 = v2.length();
	return v1.dot(v2)/(l1*l2);
}

void TorsionSpring::satisfyConstraint()
{
	double curAngle = computeAngle();
	double im2 = p2->invMass;
	double im3 = p3->invMass;
	// Handle special case
	if(im2+im3 == 0)
		return;
	// we assume that the two lenghts are equal
	double scalar = (curAngle-restAngle)/(im2+im3)*stiffness;
	Vector3 v2 = (p3->position - p1->position).cross(p3->position - p4->position).normalize();
	Vector3 v3 = -(p2->position - p1->position).cross(p2->position - p4->position).normalize();
	p3->position += v2*im2*scalar;
	p4->position += v3*im3*scalar;
}
